/*
 * Copyright (C) 2014
 *
 * Brick Yang <printfxxx@163.com>
 *
 * This program is free software. You can redistribute it and/or
 * modify it as you like.
 */

/**
 * @file	cli.c
 * @brief	Simplebits command line tools
 */

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <errno.h>
#include <netdb.h>
#include <ctype.h>
#include <fcntl.h>
#include <pcap.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <linux/ip.h>
#include <linux/tcp.h>
#include <linux/udp.h>
#include <linux/if_ether.h>
#include <linux/if_arp.h>

#include <mtrace.h>
#include <proto.h>

#define ARRAY_SIZE(arr)			(sizeof(arr) / sizeof((arr)[0]))
#define offsetof(type, member)		((size_t)&((type *)0)->member)
#define sizeof_field(type, member)	sizeof((((type *)0)->member))
#define offsetofend(type, member)	(offsetof(type, member) + sizeof_field(type, member))

struct cmd_param;
typedef struct cmd_param cmd_param_t;

typedef int (cmd_fn_t)(proto_handle_t *handle, int argc, const char *argv[], cmd_param_t *param);

typedef struct cmd {
	const char *name, *help;
	uint8_t magic, id;
	cmd_fn_t *fn;
} cmd_t;

struct cmd_param {
	cmd_t *cmd;
	void *buf;
	size_t len;
};

static const char hex_asc[] = "0123456789abcdef";

#define hex_asc_lo(x)	hex_asc[((x) & 0x0f)]
#define hex_asc_hi(x)	hex_asc[((x) & 0xf0) >> 4]

static inline char *hex_byte_pack(char *buf, uint8_t byte)
{
	*buf++ = hex_asc_hi(byte);
	*buf++ = hex_asc_lo(byte);
	return buf;
}

static int hex_to_bin(char ch)
{
	if ((ch >= '0') && (ch <= '9'))
		return ch - '0';
	ch = tolower(ch);
	if ((ch >= 'a') && (ch <= 'f'))
		return ch - 'a' + 10;
	return -1;
}

static int hex2bin(uint8_t *dst, const char *src, size_t count)
{
	while (count--) {
		int hi = hex_to_bin(*src++);
		int lo = hex_to_bin(*src++);

		if ((hi < 0) || (lo < 0))
			return -EINVAL;

		*dst++ = (hi << 4) | lo;
	}
	return 0;
}

static char *bin2hex(char *dst, const void *src, size_t count)
{
	const unsigned char *_src = src;

	while (count--)
		dst = hex_byte_pack(dst, *_src++);
	return dst;
}

static int cmd_process_std_resp(proto_handle_t *handle, proto_rxd_t *rxd, int *ret)
{
	int fd, rc;
	ssize_t n;

	while (1) {
		if ((rc = proto_recv(handle, rxd)) < 0) {
			goto err;
		}
		switch (rxd->hdr.magic) {
		case MAGIC_MSG:
			if (rxd->hdr.id == MSG_ID_STDOUT) {
				fd = STDOUT_FILENO;
			} else if (rxd->hdr.id == MSG_ID_STDERR) {
				fd = STDERR_FILENO;
			} else {
				rc = -EINVAL;
				goto err;
			}
			n = write(fd, rxd->buf, rxd->hdr.length);
			break;
		case MAGIC_RET_CODE:
			*ret = rxd->hdr.id;
			rc = 1;
			goto ok;
		default:
			rc = 0;
			goto ok;
		}
	}
ok:
err:
	return rc;
}

static int cmd_handler_no_arg(proto_handle_t *handle, int argc, const char *argv[],
			      cmd_param_t *param)
{
	int rc, ret;
	proto_txd_t txd;
	proto_rxd_t rxd;

	if (argc != 1) {
		rc = -EINVAL;
		goto err;
	}

	txd.num = 0;
	txd.hdr.magic = param->cmd->magic;
	txd.hdr.id = param->cmd->id;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_str(proto_handle_t *handle, int argc, const char *argv[],
			   cmd_param_t *param)
{
	int rc, ret;
	proto_txd_t txd;
	proto_rxd_t rxd;

	if (argc != 2) {
		rc = -EINVAL;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.num = 1;
	txd.hdr.magic = param->cmd->magic;
	txd.hdr.id = param->cmd->id;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_str_u32(proto_handle_t *handle, int argc, const char *argv[],
			       cmd_param_t *param)
{
	int rc, ret;
	__be32 be32;
	proto_txd_t txd;
	proto_rxd_t rxd;

	if (argc != 3) {
		rc = -EINVAL;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	be32 = strtoul(argv[2], NULL, 0);
	be32 = __cpu_to_be32(be32);
	txd.iov[1].iov_base = &be32;
	txd.iov[1].iov_len = sizeof(be32);
	txd.num = 2;
	txd.hdr.magic = param->cmd->magic;
	txd.hdr.id = param->cmd->id;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_str_str(proto_handle_t *handle, int argc, const char *argv[],
			       cmd_param_t *param)
{
	int rc, ret;
	proto_txd_t txd;
	proto_rxd_t rxd;

	if (argc != 3) {
		rc = -EINVAL;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)argv[2];
	txd.iov[1].iov_len = strlen(argv[2]) + 1;
	txd.num = 2;
	txd.hdr.magic = param->cmd->magic;
	txd.hdr.id = param->cmd->id;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_str_str_u32(proto_handle_t *handle, int argc, const char *argv[],
				   cmd_param_t *param)
{
	int rc, ret;
	__be32 be32;
	proto_txd_t txd;
	proto_rxd_t rxd;

	if (argc != 4) {
		rc = -EINVAL;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)argv[2];
	txd.iov[1].iov_len = strlen(argv[2]) + 1;
	be32 = strtoul(argv[3], NULL, 0);
	be32 = __cpu_to_be32(be32);
	txd.iov[2].iov_base = &be32;
	txd.iov[2].iov_len = sizeof(be32);
	txd.num = 3;
	txd.hdr.magic = param->cmd->magic;
	txd.hdr.id = param->cmd->id;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_str_str_u64(proto_handle_t *handle, int argc, const char *argv[],
				   cmd_param_t *param)
{
	int rc, ret;
	__be64 be64;
	proto_txd_t txd;
	proto_rxd_t rxd;

	if (argc != 4) {
		rc = -EINVAL;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)argv[2];
	txd.iov[1].iov_len = strlen(argv[2]) + 1;
	be64 = strtoull(argv[3], NULL, 0);
	be64 = __cpu_to_be64(be64);
	txd.iov[2].iov_base = &be64;
	txd.iov[2].iov_len = sizeof(be64);
	txd.num = 3;
	txd.hdr.magic = param->cmd->magic;
	txd.hdr.id = param->cmd->id;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_str_str_str(proto_handle_t *handle, int argc, const char *argv[],
				   cmd_param_t *param)
{
	int rc, ret;
	proto_txd_t txd;
	proto_rxd_t rxd;

	if (argc != 4) {
		rc = -EINVAL;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)argv[2];
	txd.iov[1].iov_len = strlen(argv[2]) + 1;
	txd.iov[2].iov_base = (void *)argv[3];
	txd.iov[2].iov_len = strlen(argv[3]) + 1;
	txd.num = 3;
	txd.hdr.magic = param->cmd->magic;
	txd.hdr.id = param->cmd->id;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_str_str_u32_u32_u32(proto_handle_t *handle, int argc, const char *argv[],
					   cmd_param_t *param)
{
	int i, rc, ret;
	__be32 be32[3];
	proto_txd_t txd;
	proto_rxd_t rxd;

	if (argc != 6) {
		rc = -EINVAL;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)argv[2];
	txd.iov[1].iov_len = strlen(argv[2]) + 1;
	for (i = 3; i < 6; i++) {
		be32[i - 3] = strtoul(argv[i], NULL, 0);
		be32[i - 3] = __cpu_to_be32(be32[i - 3]);
		txd.iov[i - 1].iov_base = &be32[i - 3];
		txd.iov[i - 1].iov_len = sizeof(be32[i - 3]);
	}
	txd.num = 5;
	txd.hdr.magic = param->cmd->magic;
	txd.hdr.id = param->cmd->id;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_str_str_u32_u32_u64(proto_handle_t *handle, int argc, const char *argv[],
					   cmd_param_t *param)
{
	int i, rc, ret;
	__be32 be32[2];
	__be64 be64;
	proto_txd_t txd;
	proto_rxd_t rxd;

	if (argc != 6) {
		rc = -EINVAL;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)argv[2];
	txd.iov[1].iov_len = strlen(argv[2]) + 1;
	for (i = 3; i < 5; i++) {
		be32[i - 3] = strtoul(argv[i], NULL, 0);
		be32[i - 3] = __cpu_to_be32(be32[i - 3]);
		txd.iov[i - 1].iov_base = &be32[i - 3];
		txd.iov[i - 1].iov_len = sizeof(be32[i - 3]);
	}
	be64 = strtoull(argv[5], NULL, 0);
	be64 = __cpu_to_be64(be64);
	txd.iov[4].iov_base = &be64;
	txd.iov[4].iov_len = sizeof(be64);
	txd.num = 5;
	txd.hdr.magic = param->cmd->magic;
	txd.hdr.id = param->cmd->id;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_str_or_more(proto_handle_t *handle, int argc, const char *argv[],
				   cmd_param_t *param)
{
	int i, rc, ret;
	proto_txd_t txd;
	proto_rxd_t rxd;

	if ((argc < 2) || ((argc - 1) > PROTO_IOVEC_MAX)) {
		rc = -EINVAL;
		goto err;
	}

	for (i = 1; i < argc; i++) {
		txd.iov[i - 1].iov_base = (void *)argv[i];
		txd.iov[i - 1].iov_len = strlen(argv[i]) + 1;
	}
	txd.num = argc - 1;
	txd.hdr.magic = param->cmd->magic;
	txd.hdr.id = param->cmd->id;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_no_arg_or_str(proto_handle_t *handle, int argc, const char *argv[],
				     cmd_param_t *param)
{
	switch (argc) {
	case 1:
		return cmd_handler_no_arg(handle, argc, argv, param);
	case 2:
		return cmd_handler_str(handle, argc, argv, param);
	default:
		return -EINVAL;
	}
}

enum {
	HEADER_L1_MAC = 0,
	HEADER_L2_ETH,
	HEADER_L2_VLAN,
	HEADER_L3_IPV4,
	HEADER_L3_ARP,
	HEADER_L4_TCP,
	HEADER_L4_UDP,
	DATA_1,
	DATA_2,
	DATA_3,
	DATA_4,
};

enum {
	FLAG_SKIP_IPV4_CSUM = 0,
	FLAG_SKIP_TCP_DOFF,
	FLAG_SKIP_TCP_CSUM,
	FLAG_SKIP_UDP_CSUM,
};

struct vlanhdr {
	__be16 tci;
	__be16 proto;
};

struct arp {
	struct arphdr hdr;
	unsigned char sha[ETH_ALEN];
#define IPV4_ALEN	4
	unsigned char sip[IPV4_ALEN];
	unsigned char tha[ETH_ALEN];
	unsigned char tip[IPV4_ALEN];
};

static uint64_t be_bytes_to_cpu(const void *src, size_t sz)
{
	__be64 be64 = 0;

	assert(sz <= sizeof(be64));
	memcpy((void *)&be64 + sizeof(be64) - sz, src, sz);

	return __be64_to_cpu(be64);
}

static void cpu_to_be_bytes(void *dst, uint64_t u64, size_t sz)
{
	__be64 be64;

	assert(sz <= sizeof(u64));
	be64 = __cpu_to_be64(u64);

	memcpy(dst, (const void *)&be64 + sizeof(be64) - sz, sz);
}

static inline uint16_t from32to16(uint32_t x)
{
	x = (x & 0xffff) + (x >> 16);
	x = (x & 0xffff) + (x >> 16);
	return x;
}

static inline uint32_t from64to32(uint64_t x)
{
	x = (x & 0xffffffff) + (x >> 32);
	x = (x & 0xffffffff) + (x >> 32);
	return (uint32_t)x;
}

static inline __sum16 csum_fold(__wsum csum)
{
	uint32_t sum = (uint32_t)csum;
	sum = (sum & 0xffff) + (sum >> 16);
	sum = (sum & 0xffff) + (sum >> 16);
	return (__sum16)~sum;
}

static unsigned int do_csum(const unsigned char *buff, int len)
{
	int odd;
	unsigned int result = 0;

	if (len <= 0)
		goto out;
	odd = 1 & (unsigned long) buff;
	if (odd) {
#ifdef __LITTLE_ENDIAN_BITFIELD
		result += (*buff << 8);
#else
		result = *buff;
#endif
		len--;
		buff++;
	}
	if (len >= 2) {
		if (2 & (unsigned long) buff) {
			result += *(unsigned short *) buff;
			len -= 2;
			buff += 2;
		}
		if (len >= 4) {
			const unsigned char *end = buff + ((unsigned)len & ~3);
			unsigned int carry = 0;
			do {
				unsigned int w = *(unsigned int *) buff;
				buff += 4;
				result += carry;
				result += w;
				carry = (w > result);
			} while (buff < end);
			result += carry;
			result = (result & 0xffff) + (result >> 16);
		}
		if (len & 2) {
			result += *(unsigned short *) buff;
			buff += 2;
		}
	}
	if (len & 1)
#ifdef __LITTLE_ENDIAN_BITFIELD
		result += *buff;
#else
		result += (*buff << 8);
#endif
	result = from32to16(result);
	if (odd)
		result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);
out:
	return result;
}

static __sum16 ip_fast_csum(const void *iph, unsigned int ihl)
{
	return (__sum16)~do_csum(iph, ihl * 4);
}

static __wsum csum_partial(const void *buff, int len, __wsum wsum)
{
	unsigned int sum = (unsigned int)wsum;
	unsigned int result = do_csum(buff, len);

	result += sum;
	if (sum > result)
		result += 1;
	return (__wsum)result;
}

static __wsum csum_tcpudp_nofold(__be32 saddr, __be32 daddr,
				 uint32_t len, uint8_t proto, __wsum sum)
{
	unsigned long long s = (uint32_t)sum;

	s += (uint32_t)saddr;
	s += (uint32_t)daddr;
#ifdef __BIG_ENDIAN_BITFIELD
	s += proto + len;
#else
	s += (proto + len) << 8;
#endif
	return (__wsum)from64to32(s);
}

static __sum16 csum_tcpudp_magic(__be32 saddr, __be32 daddr, uint32_t len,
				 uint8_t proto, __wsum sum)
{
	return csum_fold(csum_tcpudp_nofold(saddr, daddr, len, proto, sum));
}

static int cmd_handler_add_pkt(proto_handle_t *handle, int argc, const char *argv[],
			       cmd_param_t *param)
{
	int rc, ret;
	char op, *s, *t, *str, *dup = NULL;
	uint16_t u16;
	uint64_t u64;
	proto_txd_t txd;
	proto_rxd_t rxd;
	unsigned int i, j, header_bits = 0, flag_bits = 0;
	struct iphdr *iphdr = NULL;
	struct ethhdr *ethhdr = NULL;
	struct vlanhdr *vlanhdr = NULL;
	struct arp *arp = NULL;
	struct tcphdr *tcphdr = NULL;
	struct udphdr *udphdr = NULL;
	struct {
		void *pkt;
		uint32_t sz, len, nr, inc_off, inc_sz;
		uint64_t inc_step;
	} desc = {};
	struct {
		const char *name;
		uint32_t sz, off;
	} *p, header[] = {
		[HEADER_L1_MAC]  = {"mac",  offsetofend(struct ethhdr, h_source)},
		[HEADER_L2_ETH]  = {"eth",  sizeof(struct ethhdr)},
		[HEADER_L2_VLAN] = {"vlan", sizeof(struct vlanhdr)},
		[HEADER_L3_IPV4] = {"ipv4", sizeof(struct iphdr)},
		[HEADER_L3_ARP]  = {"arp",  sizeof(struct arp)},
		[HEADER_L4_TCP]  = {"tcp",  sizeof(struct tcphdr)},
		[HEADER_L4_UDP]  = {"udp",  sizeof(struct udphdr)},
		[DATA_1]         = {"d1",   0},
		[DATA_2]         = {"d2",   0},
		[DATA_3]         = {"d3",   0},
		[DATA_4]         = {"d4",   0},
	};
	struct {
		const char *name;
		unsigned int off, sz, flags;
	} *f, field[] = {
#define FIELD_OFF_SZ(type, member)	offsetof(type, member), sizeof_field(type, member)
		{"mac.dst",        FIELD_OFF_SZ(struct ethhdr, h_dest),                0},
		{"mac.src",        FIELD_OFF_SZ(struct ethhdr, h_source),              0},
		{"mac+",           offsetofend(struct ethhdr, h_source), 0,            0},
		{"eth.dst",        FIELD_OFF_SZ(struct ethhdr, h_dest),                0},
		{"eth.src",        FIELD_OFF_SZ(struct ethhdr, h_source),              0},
		{"eth.proto",      FIELD_OFF_SZ(struct ethhdr, h_proto),               0},
		{"eth+",           sizeof(struct ethhdr), 0,                           0},
		{"vlan.tci",       FIELD_OFF_SZ(struct vlanhdr, tci),                  0},
		{"vlan.proto",     FIELD_OFF_SZ(struct vlanhdr, proto),                0},
		{"vlan+",          sizeof(struct vlanhdr), 0,                          0},
		{"ipv4.ver_ihl",   0, sizeof(__u8),                                    0},
		{"ipv4.tos",       FIELD_OFF_SZ(struct iphdr, tos),                    0},
		{"ipv4.len",       FIELD_OFF_SZ(struct iphdr, tot_len),                0},
		{"ipv4.id",        FIELD_OFF_SZ(struct iphdr, id),                     0},
		{"ipv4.off",       FIELD_OFF_SZ(struct iphdr, frag_off),               0},
		{"ipv4.ttl",       FIELD_OFF_SZ(struct iphdr, ttl),                    0},
		{"ipv4.proto",     FIELD_OFF_SZ(struct iphdr, protocol),               0},
		{"ipv4.csum",      FIELD_OFF_SZ(struct iphdr, check),                  1 << FLAG_SKIP_IPV4_CSUM},
		{"ipv4.src",       FIELD_OFF_SZ(struct iphdr, saddr),                  0},
		{"ipv4.dst",       FIELD_OFF_SZ(struct iphdr, daddr),                  0},
		{"ipv4+",          sizeof(struct iphdr), 0,                            0},
		{"arp.htype",      FIELD_OFF_SZ(struct arp, hdr.ar_hrd),               0},
		{"arp.ptype",      FIELD_OFF_SZ(struct arp, hdr.ar_pro),               0},
		{"arp.hlen",       FIELD_OFF_SZ(struct arp, hdr.ar_hln),               0},
		{"arp.plen",       FIELD_OFF_SZ(struct arp, hdr.ar_pln),               0},
		{"arp.opcode",     FIELD_OFF_SZ(struct arp, hdr.ar_op),                0},
		{"arp.sha",        FIELD_OFF_SZ(struct arp, sha),                      0},
		{"arp.sip",        FIELD_OFF_SZ(struct arp, sip),                      0},
		{"arp.tha",        FIELD_OFF_SZ(struct arp, tha),                      0},
		{"arp.tip",        FIELD_OFF_SZ(struct arp, tip),                      0},
		{"arp+",           sizeof(struct arp), 0,                              0},
		{"tcp.src",        FIELD_OFF_SZ(struct tcphdr, source),                0},
		{"tcp.dst",        FIELD_OFF_SZ(struct tcphdr, dest),                  0},
		{"tcp.seq",        FIELD_OFF_SZ(struct tcphdr, seq),                   0},
		{"tcp.ackseq",     FIELD_OFF_SZ(struct tcphdr, ack_seq),               0},
		{"tcp.doff_flags", offsetofend(struct tcphdr, ack_seq), sizeof(__u16), 1 << FLAG_SKIP_TCP_DOFF},
		{"tcp.flags",      offsetofend(struct tcphdr, ack_seq), sizeof(__u16), 0},
		{"tcp.win",        FIELD_OFF_SZ(struct tcphdr, window),                0},
		{"tcp.csum",       FIELD_OFF_SZ(struct tcphdr, check),                 1 << FLAG_SKIP_TCP_CSUM},
		{"tcp.urgptr",     FIELD_OFF_SZ(struct tcphdr, urg_ptr),               0},
		{"tcp+",           sizeof(struct tcphdr), 0,                           0},
		{"udp.src",        FIELD_OFF_SZ(struct udphdr, source),                0},
		{"udp.dst",        FIELD_OFF_SZ(struct udphdr, dest),                  0},
		{"udp.len",        FIELD_OFF_SZ(struct udphdr, len),                   0},
		{"udp.csum",       FIELD_OFF_SZ(struct udphdr, check),                 1 << FLAG_SKIP_UDP_CSUM},
		{"udp+",           sizeof(struct udphdr), 0,                           0},
		{"d1",             0, 0,                                               0},
		{"d2",             0, 0,                                               0},
		{"d3",             0, 0,                                               0},
		{"d4",             0, 0,                                               0},
	};

	if (argc < 5) {
		fprintf(stderr, "ERR: invalid argument for \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	desc.sz = strtoul(argv[3], NULL, 0);
	if (!(desc.pkt = malloc(desc.sz)) || MTRACE_MEM_ADD(desc.pkt)) {
		fprintf(stderr, "ERR: %s(): failed to alloc memory\n", __func__);
		rc = -ENOMEM;
		goto err;
	}
	memset(desc.pkt, 0, desc.sz);

	if (!(dup = strdup(argv[4])) || MTRACE_MEM_ADD(dup)) {
		fprintf(stderr, "ERR: failed to duplicate string\n");
		rc = -ENOMEM;
		goto err;
	}
	str = dup;
	while (str) {
		t = strsep(&str, "+");
		s = strsep(&t, "@");
		for (i = 0; i < ARRAY_SIZE(header); i++) {
			if (strcmp(s, header[i].name)) {
				continue;
			}
			if ((header_bits & (1 << i))) {
				fprintf(stderr, "ERR: header \"%s\" already selected\n", s);
				rc = -EINVAL;
				goto err;
			}
			if (t) {
				header[i].sz += strtoul(t, NULL, 0);
			}
			if (desc.len + header[i].sz > desc.sz) {
				fprintf(stderr, "ERR: header size %u exceeds packet size %u\n",
					desc.len + header[i].sz, desc.sz);
				rc = -EINVAL;
				goto err;
			}
			header[i].off = desc.len;
			desc.len += header[i].sz;
			header_bits |= 1 << i;
			break;
		}
		if (i >= ARRAY_SIZE(header)) {
			fprintf(stderr, "ERR: unknown header \"%s\"\n", s);
			rc = -EINVAL;
			goto err;
		}
	}
	MTRACE_MEM_DEL(dup);
	free(dup);
	dup = NULL;

	if (header_bits & (1 << HEADER_L2_ETH)) {
		ethhdr = desc.pkt + header[HEADER_L2_ETH].off;
		if (header_bits & (1 << HEADER_L3_IPV4)) {
			ethhdr->h_proto = htons(ETH_P_IP);
		} else if (header_bits & (1 << HEADER_L3_ARP)) {
			ethhdr->h_proto = htons(ETH_P_ARP);
		}
	}

	if (header_bits & (1 << HEADER_L2_VLAN)) {
		vlanhdr = desc.pkt + header[HEADER_L2_VLAN].off;
		if (header_bits & (1 << HEADER_L2_ETH)) {
			vlanhdr->proto = ethhdr->h_proto;
			ethhdr->h_proto = htons(ETH_P_8021Q);
		}
	}

	if (header_bits & (1 << HEADER_L3_ARP)) {
		arp = desc.pkt + header[HEADER_L3_ARP].off;
		if (header_bits & (1 << HEADER_L2_ETH)) {
			arp->hdr.ar_hrd = htons(ARPHRD_ETHER);
			arp->hdr.ar_hln = ETH_ALEN;
		}
	}

	if (header_bits & (1 << HEADER_L3_IPV4)) {
		iphdr = desc.pkt + header[HEADER_L3_IPV4].off;
		iphdr->version = IPVERSION;
		iphdr->ihl = header[HEADER_L3_IPV4].sz >> 2;
		u16 = desc.len - header[HEADER_L3_IPV4].off;
		iphdr->tot_len = htons(u16);
		if (header_bits & (1 << HEADER_L4_TCP)) {
			iphdr->protocol = IPPROTO_TCP;
		} else if (header_bits & (1 << HEADER_L4_UDP)) {
			iphdr->protocol = IPPROTO_UDP;
		}
	}

	if (header_bits & (1 << HEADER_L4_TCP)) {
		tcphdr = desc.pkt + header[HEADER_L4_TCP].off;
	}

	if (header_bits & (1 << HEADER_L4_UDP)) {
		udphdr = desc.pkt + header[HEADER_L4_UDP].off;
		u16 = desc.len - header[HEADER_L4_UDP].off;
		udphdr->len = htons(u16);
	}

	for (i = 5; i < argc; i++) {
		if (!(dup = strdup(argv[i])) || MTRACE_MEM_ADD(dup)) {
			fprintf(stderr, "ERR: failed to duplicate string\n");
			rc = -ENOMEM;
			goto err;
		}
		s = dup;
		if (!(str = strpbrk(s, "=:"))) {
			fprintf(stderr, "ERR: bad expression \"%s\"\n", s);
			rc = -EINVAL;
			goto err;
		}
		op = *str;
		*str++ = '\0';
		for (j = 0; j < ARRAY_SIZE(field); j++) {
			if (strcmp(s, field[j].name)) {
				continue;
			}
			f = &field[j];
			break;
		}
		if (j >= ARRAY_SIZE(field)) {
			fprintf(stderr, "ERR: unknown field \"%s\"\n", s);
			rc = -EINVAL;
			goto err;
		}
		if ((t = strpbrk(s, ".+"))) {
			*t = '\0';
		}
		for (j = 0; j < ARRAY_SIZE(header); j++) {
			if (strcmp(s, header[j].name)) {
				continue;
			}
			p = &header[j];
			break;
		}
		if (j >= ARRAY_SIZE(header)) {
			fprintf(stderr, "ERR: unknown header \"%s\"\n", s);
			rc = -EINVAL;
			goto err;
		}
		if (!(header_bits & (1 << j))) {
			fprintf(stderr, "ERR: header \"%s\" not select\n", p->name);
			rc = -EINVAL;
			goto err;
		}
		if (!f->sz) {
			f->sz = p->sz - f->off;
		}
		flag_bits |= f->flags;
		if (op == '=') {
			s = strsep(&str, ":");
			u64 = strtoull(s, NULL, 0);
			cpu_to_be_bytes(desc.pkt + p->off + f->off, u64, f->sz);
			if ((s = strsep(&str, ":"))) {
				desc.nr = strtoul(s, NULL, 0);
				desc.inc_off = p->off + f->off;
				desc.inc_sz = f->sz;
				desc.inc_step = 1;
			}
			if (str) {
				desc.inc_step = strtoull(str, NULL, 0);
			}
		} else {
			j = strlen(str) >> 1;
			if (j > f->sz) {
				fprintf(stderr, "ERR: field \"%s\" size %u exceeds the limit %u\n",
					f->name, j, f->sz);
				rc = -EINVAL;
				goto err;
			}
			if ((rc = hex2bin(desc.pkt + p->off + f->off, str, j))) {
				goto err;
			}
		}
		MTRACE_MEM_DEL(dup);
		free(dup);
		dup = NULL;
	}

	if (header_bits & (1 << HEADER_L4_TCP)) {
		if (!(flag_bits & (1 << FLAG_SKIP_TCP_DOFF))) {
			tcphdr->doff = header[HEADER_L4_TCP].sz >> 2;
		}
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)argv[2];
	txd.iov[1].iov_len = strlen(argv[2]) + 1;
	txd.num = 2;
	txd.hdr.magic = MAGIC_WORKER;
	txd.hdr.id = WORKER_ID_ADD_PKT;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	desc.nr = desc.nr ? : 1;
	for (i = 0; i < desc.nr; i++) {
		if (header_bits & (1 << HEADER_L3_IPV4)) {
			if (!(flag_bits & (1 << FLAG_SKIP_IPV4_CSUM))) {
				iphdr->check = 0;
				iphdr->check = ip_fast_csum(iphdr, iphdr->ihl);
			}
			if (header_bits & (1 << HEADER_L4_TCP)) {
				if (!(flag_bits & (1 << FLAG_SKIP_TCP_CSUM))) {
					u16 = desc.len - header[HEADER_L4_TCP].off;
					tcphdr->check = 0;
					tcphdr->check = csum_tcpudp_magic(iphdr->saddr, iphdr->daddr, u16, IPPROTO_TCP,
									  csum_partial(tcphdr, u16, 0));
				}
			} else if (header_bits & (1 << HEADER_L4_UDP)) {
				if (!(flag_bits & (1 << FLAG_SKIP_UDP_CSUM))) {
					u16 = desc.len - header[HEADER_L4_UDP].off;
					udphdr->check = 0;
					udphdr->check = csum_tcpudp_magic(iphdr->saddr, iphdr->daddr, u16, IPPROTO_UDP,
									  csum_partial(udphdr, u16, 0));
					udphdr->check = udphdr->check ? : ~udphdr->check;
				}
			}
		}
		txd.iov[0].iov_base = desc.pkt;
		txd.iov[0].iov_len = desc.sz;
		txd.hdr.magic = MAGIC_PRIV;
		txd.num = 1;
		if ((rc = proto_send(handle, &txd)) < 0) {
			goto err;
		}
		u64 = be_bytes_to_cpu(desc.pkt + desc.inc_off, desc.inc_sz) + desc.inc_step;
		cpu_to_be_bytes(desc.pkt + desc.inc_off, u64, desc.inc_sz);
	}

	txd.hdr.magic = MAGIC_PRIV;
	txd.num = 0;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	rc = ret;
	goto ok;
err:
ok:
	MTRACE_MEM_DEL(dup);
	free(dup);
	MTRACE_MEM_DEL(desc.pkt);
	free(desc.pkt);
	return rc;
}

static int cmd_handler_dump_pkt(proto_handle_t *handle, int argc, const char *argv[],
				cmd_param_t *param)
{
	int rc, ret;
	size_t n;
	unsigned int i;
	char *str = NULL;
	proto_txd_t txd;
	proto_rxd_t rxd;

	if (argc < 3) {
		fprintf(stderr, "ERR: invalid argument for \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	if (!(str = malloc(PROTO_BUF_MAX * 2 + 1)) || MTRACE_MEM_ADD(str)) {
		fprintf(stderr, "ERR: %s(): failed to alloc memory\n", __func__);
		rc = -ENOMEM;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)argv[2];
	txd.iov[1].iov_len = strlen(argv[2]) + 1;
	txd.num = 2;
	txd.hdr.magic = MAGIC_WORKER;
	txd.hdr.id = WORKER_ID_DUMP_PKT;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	i = 0;
	while (1) {
		rxd.buf = param->buf;
		rxd.len = param->len;
		if ((rc = cmd_process_std_resp(handle, &rxd, &ret))) {
			break;
		}
		if (rxd.hdr.magic == MAGIC_PRIV && rxd.hdr.length) {
			n = (rxd.hdr.length > PROTO_BUF_MAX) ? PROTO_BUF_MAX : rxd.hdr.length;
			*bin2hex(str, rxd.buf, n) = '\0';
			printf("pkt[%u]: len=%u\n%s\n", i++, rxd.hdr.length, str);
		}
	}

	if (rc < 0) {
		goto err;
	}

	rc = ret;
	goto ok;
err:
ok:
	MTRACE_MEM_DEL(str);
	free(str);
	return rc;
}

static int cmd_handler_load_pkt(proto_handle_t *handle, int argc, const char *argv[],
				cmd_param_t *param)
{
	int rc, ret;
	char err_buf[PCAP_ERRBUF_SIZE];
	pcap_t *pcap = NULL;
	proto_txd_t txd;
	proto_rxd_t rxd;
	const u_char *pkt;
	struct pcap_pkthdr hdr;

	if (argc < 4) {
		fprintf(stderr, "ERR: invalid argument for \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	if (!(pcap = pcap_open_offline(argv[3], err_buf))) {
		fprintf(stderr, "ERR: failed to open file \"%s\"\n", argv[3]);
		rc = -ENOENT;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)argv[2];
	txd.iov[1].iov_len = strlen(argv[2]) + 1;
	txd.num = 2;
	txd.hdr.magic = MAGIC_WORKER;
	txd.hdr.id = WORKER_ID_ADD_PKT;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	while ((pkt = pcap_next(pcap, &hdr))) {
		txd.iov[0].iov_base = (void *)pkt;
		txd.iov[0].iov_len = hdr.len;
		txd.hdr.magic = MAGIC_PRIV;
		txd.num = 1;
		if ((rc = proto_send(handle, &txd)) < 0) {
			goto err;
		}
	}

	txd.hdr.magic = MAGIC_PRIV;
	txd.num = 0;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	rc = ret;
	goto ok;
err:
ok:
	if (pcap) {
		pcap_close(pcap);
	}

	return rc;
}

static int cmd_handler_save_pkt(proto_handle_t *handle, int argc, const char *argv[],
				cmd_param_t *param)
{
	int rc, ret;
	pcap_t *pcap = NULL;
	proto_txd_t txd;
	proto_rxd_t rxd;
	pcap_dumper_t *dump = NULL;
	struct pcap_pkthdr hdr;

	if (argc < 4) {
		fprintf(stderr, "ERR: invalid argument for \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	if (!(pcap = pcap_open_dead(DLT_EN10MB, 0x40000))) {
		rc = -ENOMEM;
		goto err;
	}
	if (!(dump = pcap_dump_open(pcap, argv[3]))) {
		fprintf(stderr, "ERR: failed to open file \"%s\"\n", argv[3]);
		rc = -EPERM;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)argv[2];
	txd.iov[1].iov_len = strlen(argv[2]) + 1;
	txd.num = 2;
	txd.hdr.magic = MAGIC_WORKER;
	txd.hdr.id = WORKER_ID_DUMP_PKT;
	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	while (1) {
		rxd.buf = param->buf;
		rxd.len = param->len;
		if ((rc = cmd_process_std_resp(handle, &rxd, &ret))) {
			break;
		}
		if (rxd.hdr.magic == MAGIC_PRIV && rxd.hdr.length) {
			hdr.ts = (struct timeval){};
			hdr.len = rxd.hdr.length;
			hdr.caplen = rxd.hdr.length;
			pcap_dump((u_char *)dump, &hdr, rxd.buf);
		}
	}

	if (rc < 0) {
		goto err;
	}

	rc = ret;
	goto ok;
err:
ok:
	if (dump) {
		pcap_dump_close(dump);
	}
	if (pcap) {
		pcap_close(pcap);
	}

	return rc;
}

static int cmd_handler_pause(proto_handle_t *handle, int argc, const char *argv[],
			     cmd_param_t *param)
{
	int rc, ret;
	char *s, *str, *dup;
	proto_txd_t txd;
	proto_rxd_t rxd;
	unsigned int i, j;
	const char *option[] = {
		[NETDEV_PAUSE_OPT_AUTONEG] = "autoneg",
		[NETDEV_PAUSE_OPT_RX] = "rx",
		[NETDEV_PAUSE_OPT_TX] = "tx",
	};
	__be32 opts = 0, be32[ARRAY_SIZE(option)];

	if (argc < 3) {
		fprintf(stderr, "ERR: invalid argument for \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)&opts;
	txd.iov[1].iov_len = sizeof(opts);
	for (i = 0; i < ARRAY_SIZE(option); i++) {
		be32[i] = __cpu_to_be32(0);
		txd.iov[2 + i].iov_base = &be32[i];
		txd.iov[2 + i].iov_len = sizeof(be32[i]);
	}
	txd.num = 2 + ARRAY_SIZE(option);
	txd.hdr.magic = MAGIC_NETDEV;
	txd.hdr.id = NETDEV_ID_PAUSE;

	for (i = 2; i < argc; i++) {
		if (!(dup = strdup(argv[i])) || MTRACE_MEM_ADD(dup)) {
			fprintf(stderr, "ERR: failed to duplicate string\n");
			rc = -ENOMEM;
			goto err;
		}
		str = dup;
		s = strsep(&str, "=");
		if (!str) {
			fprintf(stderr, "ERR: bad expression \"%s\"\n", s);
			rc = -EINVAL;
			MTRACE_MEM_DEL(dup);
			free(dup);
			goto err;
		}

		for (j = 0; j < ARRAY_SIZE(option); j++) {
			if (strcmp(s, option[j])) {
				continue;
			}
			be32[j] = strtoul(str, NULL, 0);
			be32[j] = __cpu_to_be32(be32[j]);
			opts |= __cpu_to_be32(1 << j);
		}
		MTRACE_MEM_DEL(dup);
		free(dup);
	}

	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static int cmd_handler_link(proto_handle_t *handle, int argc, const char *argv[],
			    cmd_param_t *param)
{
	int rc, ret;
	char *s, *str, *dup;
	proto_txd_t txd;
	proto_rxd_t rxd;
	unsigned int i, j;
	const char *option[] = {
		[NETDEV_LINK_OPT_AUTONEG] = "autoneg",
		[NETDEV_LINK_OPT_DUPLEX]  = "duplex",
		[NETDEV_LINK_OPT_SPEED]   = "speed",
		[NETDEV_LINK_OPT_ADVSET]  = "advset",
		[NETDEV_LINK_OPT_ADVCLR]  = "advclr",
	};
	__be32 opts = 0, be32[ARRAY_SIZE(option)];

	if (argc < 3) {
		fprintf(stderr, "ERR: invalid argument for \"%s\"\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	txd.iov[0].iov_base = (void *)argv[1];
	txd.iov[0].iov_len = strlen(argv[1]) + 1;
	txd.iov[1].iov_base = (void *)&opts;
	txd.iov[1].iov_len = sizeof(opts);
	for (i = 0; i < ARRAY_SIZE(option); i++) {
		switch (i) {
		case NETDEV_LINK_OPT_ADVSET:
		case NETDEV_LINK_OPT_ADVCLR:
			str = (char *)&be32[i];
			*str = '\0';
			txd.iov[2 + i].iov_base = str;
			txd.iov[2 + i].iov_len = strlen(str) + 1;
			break;

		default:
			be32[i] = __cpu_to_be32(0);
			txd.iov[2 + i].iov_base = &be32[i];
			txd.iov[2 + i].iov_len = sizeof(be32[i]);
			break;
		}
	}
	txd.num = 2 + ARRAY_SIZE(be32);
	txd.hdr.magic = MAGIC_NETDEV;
	txd.hdr.id = NETDEV_ID_LINK;

	for (i = 2; i < argc; i++) {
		if (!(dup = strdup(argv[i])) || MTRACE_MEM_ADD(dup)) {
			fprintf(stderr, "ERR: failed to duplicate string\n");
			rc = -ENOMEM;
			goto err;
		}
		str = dup;
		s = strsep(&str, "=");
		if (!str) {
			fprintf(stderr, "ERR: bad expression \"%s\"\n", s);
			rc = -EINVAL;
			MTRACE_MEM_DEL(dup);
			free(dup);
			goto err;
		}

		for (j = 0; j < ARRAY_SIZE(option); j++) {
			if (strcmp(s, option[j])) {
				continue;
			}
			switch (j) {
			case NETDEV_LINK_OPT_ADVSET:
			case NETDEV_LINK_OPT_ADVCLR:
				str = (void *)argv[i] + (str - dup);
				txd.iov[2 + j].iov_base = str;
				txd.iov[2 + j].iov_len = strlen(str) + 1;
				break;

			default:
				be32[j] = strtoul(str, NULL, 0);
				be32[j] = __cpu_to_be32(be32[j]);
				break;
			}
			opts |= __cpu_to_be32(1 << j);
		}
		MTRACE_MEM_DEL(dup);
		free(dup);
	}

	if ((rc = proto_send(handle, &txd)) < 0) {
		goto err;
	}

	rxd.buf = param->buf;
	rxd.len = param->len;
	while (!(rc = cmd_process_std_resp(handle, &rxd, &ret))) {
		continue;
	}

	if (rc < 0) {
		goto err;
	}

	return ret;
err:
	return rc;
}

static cmd_t cmd_tbl[] = {
	{"worker",   "Get worker list or status of worker\n"
	             "%s %s < cpu >\n",
	             MAGIC_WORKER, WORKER_ID_WORKER, cmd_handler_no_arg_or_str},
	{"fc",       "Set flow control parameters of netdev\n"
	             "%s %s [ cpu ] [ netdev ] [ mode ] [ burst ] [ rate ]\n"
	             "\tmode  - 0 for pkts/sec, 1 for bytes/sec\n"
	             "\tburst - number of packets or bytes for each transmission\n"
	             "\trate  - packets or bytes limitation per second, 0 for disable\n",
	             MAGIC_WORKER, WORKER_ID_FC, cmd_handler_str_str_u32_u32_u64},
	{"queue",    "Set queue of netdev\n"
	             "%s %s [ cpu ] [ netdev ] [ qlen ] [ qth ] [ qwt ]\n"
	             "\tqlen - length of queue\n"
	             "\tqth  - threshold of queue. Low watermark threshold for enqueue\n"
	             "\tqwt  - weight of queue. Maximum number of packets for dequeue\n",
	             MAGIC_WORKER, WORKER_ID_QUEUE, cmd_handler_str_str_u32_u32_u32},
	{"pool_sz",  "Set pool size\n"
	             "%s %s [ cpu ] [ netdev ] [ sz ]\n",
	             MAGIC_WORKER, WORKER_ID_POOL_SZ, cmd_handler_str_str_u32},
	{"pkt_cnt",  "Set packets count\n"
	             "%s %s [ cpu ] [ netdev ] [ count ]\n",
	             MAGIC_WORKER, WORKER_ID_PKT_CNT,  cmd_handler_str_str_u64},
	{"add_pkt",  "add pkts\n"
	             "%s %s [ cpu ] [ netdev ] [ sz ] [ proto ] < ... >\n",
	             0, 0, cmd_handler_add_pkt},
	{"del_pkt",  "Delete pkts\n"
	             "%s %s [ cpu ] [ netdev ]\n",
	             MAGIC_WORKER, WORKER_ID_DEL_PKT, cmd_handler_str_str},
	{"dump_pkt", "Dump pkts\n"
	             "%s %s [ cpu ] [ netdev ]\n",
	             0, 0, cmd_handler_dump_pkt},
	{"pkt_txq",  "Set txq mapping of pkts\n"
	             "%s %s [ cpu ] [ netdev ] [ txq ]\n",
	             MAGIC_WORKER, WORKER_ID_PKT_TXQ, cmd_handler_str_str_str},
	{"load_pkt", "Load pkts from pcap file\n"
	             "%s %s [ cpu ] [ netdev ] [ filename ]\n",
	             0, 0, cmd_handler_load_pkt},
	{"save_pkt", "save pkts to pcap file\n"
	             "%s %s [ cpu ] [ netdev ] [ filename ]\n",
	             0, 0, cmd_handler_save_pkt},
	{"netdev",   "Get netdev list or status of netdev\n"
	             "%s %s < netdev >\n",
	             MAGIC_NETDEV, NETDEV_ID_NETDEV, cmd_handler_no_arg_or_str},
	{"pause",    "Configure pause setting of netdev\n"
	             "%s %s [ netdev ] < ... >\n",
	             0, 0, cmd_handler_pause},
	{"link",     "Configure link setting of netdev\n"
	             "%s %s [ netdev ] < ... >\n",
	             0, 0, cmd_handler_link},
	{"start",    "Start traffic on netdev\n"
	             "%s %s [ netdev ]\n",
	             MAGIC_NETDEV, NETDEV_ID_START, cmd_handler_str},
	{"stop",     "Stop traffic on netdev\n"
	             "%s %s [ netdev ]\n",
	             MAGIC_NETDEV, NETDEV_ID_STOP, cmd_handler_str},
	{"clear",    "Clear statistics of netdev\n"
	             "%s %s [ netdev ]\n",
	             MAGIC_NETDEV, NETDEV_ID_CLEAR, cmd_handler_str},
	{"stats",    "Get statistics of all netdev\n"
	             "%s %s\n",
	             MAGIC_NETDEV, NETDEV_ID_STATS, cmd_handler_no_arg},
	{"attach",   "Attach netdev\n"
	             "%s %s [ netdev ]\n",
	             MAGIC_NETDEV, NETDEV_ID_ATTACH, cmd_handler_str},
	{"detach",   "Detach netdev\n"
	             "%s %s [ netdev ]\n",
	             MAGIC_NETDEV, NETDEV_ID_DETACH, cmd_handler_str},
	{"rxdump",   "Set rxdump on netdev, 0 for disable, 1 for enable\n"
	             "%s %s [ netdev ] [ val ]\n",
	             MAGIC_NETDEV, NETDEV_ID_RXDUMP, cmd_handler_str_u32},
	{"devstats", "Use netdev statistics, 0 for disable, 1 for enable\n"
	             "%s %s [ netdev ] [ val ]\n",
	             MAGIC_NETDEV, NETDEV_ID_DEVSTATS, cmd_handler_str_u32},
	{"ioctl",    "Send ioctl cmd to private netdev\n"
	             "%s %s [ netdev ] [ cmds ]\n",
	             MAGIC_NETDEV, NETDEV_ID_IOCTL, cmd_handler_str_or_more},
};

static int process_cmd_from_arg(proto_handle_t *handle, int argc, const char *argv[])
{
	int i, rc;
	cmd_param_t param = {};

	if (!(param.buf = malloc(PROTO_BUF_MAX)) || MTRACE_MEM_ADD(param.buf)) {
		fprintf(stderr, "ERR: %s(): failed to alloc memory\n", __func__);
		rc = -ENOMEM;
		goto err;
	}
	param.len = PROTO_BUF_MAX;

	for (i = 0; i < ARRAY_SIZE(cmd_tbl); i++) {
		if (!strcmp(argv[0], cmd_tbl[i].name)) {
			param.cmd = &cmd_tbl[i];
			break;
		}
	}

	if (!param.cmd) {
		fprintf(stderr, "ERR: command \"%s\" not found\n", argv[0]);
		rc = -EINVAL;
		goto err;
	}

	if ((rc = param.cmd->fn(handle, argc, argv, &param))) {
		goto err;
	}

	rc = 0;
	goto ok;
ok:
err:
	MTRACE_MEM_DEL(param.buf);
	free(param.buf);
	return rc;
}

static void show_help(void)
{
	int i;

	printf("SimpleBits CLI\n");
	printf("Usage:\n");
	printf("sb_cli [ -h server ip ] [ -p server port] commands...\n");
	printf("Default server ip: 127.0.0.1\n");
	printf("Default server port: 1234\n");
	printf("\nCommands:\n");

	for (i = 0; i < ARRAY_SIZE(cmd_tbl); i++) {
		printf(cmd_tbl[i].help, "sb_cli", cmd_tbl[i].name);
		printf("\n");
	}
}

static ssize_t cmd_ops_sendmsg(proto_sock_t sock, proto_iovec_t *iov, size_t num, int flags)
{
	ssize_t ret;
	struct msghdr msg = {};

	msg.msg_iov = iov;
	msg.msg_iovlen = num;

	if ((ret = sendmsg(sock, &msg, 0)) == -1) {
		return -EIO;
	} else {
		return ret;
	}
}

static ssize_t cmd_ops_recv(proto_sock_t sock, void *buf, size_t len, int flags)
{
	ssize_t ret;

	if ((ret = recv(sock, buf, len, flags)) == -1) {
		return -EIO;
	} else {
		return ret;
	}
}

int main(int argc, const char *argv[])
{
	int i, rc, sockfd = -1;
	char *s, *ip_env;
	uint16_t port = 1234;
	const char *ip = "127.0.0.1";
	proto_handle_t handle = {};
	struct sockaddr_in sin;

	if ((rc = mtrace_init())) {
		goto err_mtrace;
	}

	if ((ip_env = getenv("SB_IP"))) {
		s = strsep(&ip_env, ":");
		if (strlen(s)) {
			ip = s;
		}
		if (ip_env) {
			port = strtoul(ip_env, NULL, 0);
		}
	}

	for (i = 1; i < argc; i++) {
		if (!strcmp(argv[i], "-h")) {
			if (++i == argc) {
				show_help();
				rc = -EINVAL;
				goto err;
			}
			ip = argv[i];
		} else if (!strcmp(argv[i], "-p")) {
			if (++i == argc) {
				show_help();
				rc = -EINVAL;
				goto err;
			}
			port = strtoul(argv[i], NULL, 0);
		} else {
			break;
		}
	}

	handle.sendmsg = cmd_ops_sendmsg;
	handle.recv = cmd_ops_recv;

	memset(&sin, 0, sizeof(sin));
	sin.sin_family = AF_INET;
	sin.sin_addr.s_addr = inet_addr(ip);
	sin.sin_port = htons(port);

	if ((sockfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) == -1) {
		fprintf(stderr, "ERR: failed to create socket\n");
		rc = -ENODEV;
		goto err;
	}

	if (connect(sockfd, (struct sockaddr *)&sin, sizeof(struct sockaddr)) == -1) {
		fprintf(stderr, "ERR: failed to connect socket\n");
		rc = -EIO;
		goto err;
	}

	handle.sock = sockfd;
	handle.io_err = false;

	if (i >= argc) {
		show_help();
		rc = -EINVAL;
		goto err;
	}

	rc = process_cmd_from_arg(&handle, argc - i, &argv[i]);
	rc = rc < 0 ? -rc : rc;
	if (rc) {
		fprintf(stderr, "ERR: %s\n", strerror(rc));
		goto err;
	}

	shutdown(sockfd, SHUT_RDWR);
	close(sockfd);
	mtrace_finish();
	return 0;
err:
	if (sockfd >= 0) {
		shutdown(sockfd, SHUT_RDWR);
		close(sockfd);
	}
	mtrace_finish();
err_mtrace:
	errno = -rc;
	return -1;
}
